﻿using System;
using System.Text;
using System.Drawing;
using System.Buffers;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;

public static partial class glDRIVE
{
    /*
    函数 gl.kalman
    Kalman滤波
    int kalman(int n, int m, int k, double f[], double q[], double r[],	double h[], double y[], double x[], double p[], double g[])
    参数 n: 动态系统的维数。
    参数 m: 观测系统的维数
    参数 k: 观测序列长度。
    参数 f: f[n][n]系统状态转移矩阵
    参数 q: q[n][n]模型噪声W的协方差阵。
    参数 r: r[m][m]观测噪声V的协方差阵。
    参数 h: h[m][n]观测矩阵
    参数 y: y[k][m]观测向量序列。
    参数 x: x[k][n]x[0][j]存放初值。其余各行返回状态向量估值序列。
    参数 p: p[n][n]存放初值。返回最后时刻的估计误差协方差阵。
    参数 g: g[n][m]返回最后时刻的稳定增益矩阵。
    返回值 函数返回标志值。若=0表示求逆失败，若不为0表示正常。
    */

    public static string drive_kalman()
    {
        Random rnd = new Random();
        int i, j, js;
        double t, s;
        double[,] p = new double[3, 3];
        double[,] x = new double[150, 3];
        double[,] y = new double[150, 1];
        double[,] g = new double[3, 1];
        double[,] f = new double[3, 3]{
            {1.0,0.05,0.00125},
            {0.0,1.0,0.05},
            {0.0,0.0,1.0}
        };
        double[,] q = new double[3, 3]{
            {0.25,0.0,0.0},
            {0.0,0.25,0.0},
            {0.0,0.0,0.25}
        };
        double[,] r = new double[1, 1] {
            { 0.25 }
        };
        double[,] h = new double[1, 3] {
            { 1.0, 0.0, 0.0 }
        };
        for (i = 0; i <= 2; i++)
            for (j = 0; j <= 2; j++) p[i, j] = 0.0;
        for (i = 0; i <= 149; i++)
            for (j = 0; j <= 2; j++) x[i, j] = 0.0;

        // 产生150个均值为0，方差为0.25的高斯白噪声
        for (i = 0; i < 149; i++)
            y[i, 0] = rnd.NextDouble() * 0.5;
        for (i = 0; i <= 149; i++)
        {
            t = 0.05 * i;
            y[i, 0] = 5.0 - 2.0 * t + 3.0 * t * t + y[i, 0];
        }

        js = gl.kalman(3, 1, 150, f, q, r, h, y, x, p, g);
        if (js == 0) return "error: 0";

        string rs = "";
        rs += gl.html_table("X", x);
        rs += gl.html_table("y", y);
        rs += gl.html_table("p", p);
        rs += gl.html_table("g", g);
        rs += gl.html_table("f", f);
        rs += gl.html_table("r", r);
        rs += gl.html_table("h", h);

        double[,] w = new double[30, 6];
        j = 0;
        for (i = 0; i <= 149; i = i + 5, j++)
        {
            t = 0.05 * i;
            s = 5.0 - 2.0 * t + 3.0 * t * t;
            w[j, 0] = t;
            w[j, 1] = s;
            w[j, 2] = y[i, 0];
            w[j, 3] = x[i, 0];
            w[j, 4] = x[i, 1];
            w[j, 5] = x[i, 2];
        }
        rs += gl.html_table("w", w);

        return rs;
        /*
        // cout <<setw(5) <<"t" <<setw(10) <<"s"  <<setw(10) <<"y"  << setw(10) << "x(0)" << setw(10) << "x(1)" << setw(10) << "x(2)" << endl;
        for (i = 0; i <= 149; i = i + 5)
        {
            t = 0.05 * i;
            s = 5.0 - 2.0 * t + 3.0 * t * t;
            // cout <<setw(5) <<t <<setw(10) <<s << setw(10) << y[i, 0] << setw(10) << x[i, 0] << setw(10) << x[i, 1] << setw(10) << x[i, 2] << endl;
        }
        return "error: 0";
        */
    }
}